#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Bool.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/cache.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/time_synchronizer.h>

#include "feature_tracker.h"

#define SHOW_UNDISTORTION 0

vector<uchar> r_status;
vector<float> r_err;
queue<sensor_msgs::ImageConstPtr> img_buf; // 图像缓存

ros::Publisher pub_img;         // 发布特征跟踪
ros::Publisher pub_match;       // 发布特征匹配
ros::Publisher pub_match_event; // 发布特征匹配
ros::Publisher pub_restart;     // 发布restart_flag

FeatureTracker trackerData[NUM_OF_CAM]; // 每个相机都有一个FeatureTracker实例，即trackerData[i]

double first_image_time;      // 第一帧图像的时间戳
int pub_count = 1;            // 发布计数
bool first_image_flag = true; // 第一帧
double last_image_time = 0;   // 上一帧图像的时间戳
bool init_pub = 0;            // 发布第一帧

/**
 * @brief   ROS的回调函数，对新来的图像进行特征点追踪，发布
 * @Description readImage()函数对新来的图像使用光流法进行特征点跟踪
 *              追踪的特征点封装成feature_points发布到pub_img的话题下，
 *              图像封装成ptr发布在pub_match下
 * @param[in]   img_msg 输入的图像
 * @return      void
 */
void imgCb(const sensor_msgs::ImageConstPtr &img_msg, const sensor_msgs::ImageConstPtr &event_msg)
{
    ROS_INFO("=========Entering imgCb function===========");
    // 判断是否为第一帧
    if (first_image_flag)
    {
        first_image_flag = false;
        first_image_time = img_msg->header.stamp.toSec();
        last_image_time = img_msg->header.stamp.toSec();
        return;
    }

    // 通过时间间隔判断相机数据流是否稳定，有问题则restart
    if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
    {
        ROS_WARN("image discontinue! reset the feature tracker!");
        first_image_flag = true;
        last_image_time = 0;
        pub_count = 1;
        std_msgs::Bool restart_flag;
        restart_flag.data = true;
        pub_restart.publish(restart_flag);
        return;
    }
    last_image_time = img_msg->header.stamp.toSec();

    // 控制发布频率
    if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
    {
        PUB_THIS_FRAME = true;
        // 时间间隔内的发布频率十分接近设定频率时，更新时间间隔起始时刻，并将数据发布次数置0
        if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
        {
            first_image_time = img_msg->header.stamp.toSec();
            pub_count = 0;
        }
    }
    else
        PUB_THIS_FRAME = false; // 不发布

    ROS_INFO("=========Feature detect begin!===========");
    cv_bridge::CvImageConstPtr ptr;
    ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
    cv::Mat show_img = ptr->image;
    cv_bridge::CvImageConstPtr e_ptr;
    e_ptr = cv_bridge::toCvCopy(event_msg, sensor_msgs::image_encodings::MONO8);
    cv::Mat e_show_img = e_ptr->image;
    // TicToc t_r;
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ROS_DEBUG("processing camera %d", i);
        // readImage()函数读取图像数据进行处理
        if (i == 0)
            // trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
            trackerData[i].readImage(ptr->image.rowRange(0, ROW), img_msg->header.stamp.toSec());
        else if (i == 1)
            // trackerData[i].readImage(e_ptr->image.rowRange(ROW * i, ROW * (i + 1)), event_msg->header.stamp.toSec());
            trackerData[i].readImage(e_ptr->image.rowRange(0, ROW), event_msg->header.stamp.toSec());
        // trackerData[i].readImage(e_ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec(), readImageType::EVENT);
    }

    ROS_INFO("=========Feature detected!===========");
    // 更新全局ID
    for (unsigned int i = 0;; i++)
    {
        bool completed = false;
        for (int j = 0; j < NUM_OF_CAM; j++)
            // if (j != 1 || !STEREO_TRACK)
            completed |= trackerData[j].updateID(i);

        if (!completed) // completed为false
            // frame_features = i - 1; // 0~i-1为图像特征
            break;
    }

    ROS_INFO("=========ID updated!===========");
    // 1. 将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，封装成sensor_msgs::PointCloudPtr类型的feature_points实例中, 发布到pub_img;
    // 2. 将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match
    if (PUB_THIS_FRAME)
    {
        pub_count++;
        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
        sensor_msgs::ChannelFloat32 id_of_point;
        sensor_msgs::ChannelFloat32 u_of_point;
        sensor_msgs::ChannelFloat32 v_of_point;
        sensor_msgs::ChannelFloat32 velocity_x_of_point;
        sensor_msgs::ChannelFloat32 velocity_y_of_point;

        feature_points->header = img_msg->header;
        feature_points->header.frame_id = "world";

        vector<set<int>> hash_ids(NUM_OF_CAM);
        for (int i = 0; i < NUM_OF_CAM; i++)
        {
            auto &un_pts = trackerData[i].cur_un_pts;         // 归一化平面
            auto &cur_pts = trackerData[i].cur_pts;           // 像素平面
            auto &ids = trackerData[i].ids;                   // 像素ID
            auto &pts_velocity = trackerData[i].pts_velocity; // 像素速度
            for (unsigned int j = 0; j < ids.size(); j++)
            {
                if (trackerData[i].track_cnt[j] > 1)
                {
                    int p_id = ids[j];
                    hash_ids[i].insert(p_id);
                    geometry_msgs::Point32 p;
                    p.x = un_pts[j].x;
                    p.y = un_pts[j].y;
                    p.z = 1;

                    feature_points->points.push_back(p);
                    id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
                    u_of_point.values.push_back(cur_pts[j].x);
                    v_of_point.values.push_back(cur_pts[j].y);
                    velocity_x_of_point.values.push_back(pts_velocity[j].x);
                    velocity_y_of_point.values.push_back(pts_velocity[j].y);
                }
            }
        }
        feature_points->channels.push_back(id_of_point);
        feature_points->channels.push_back(u_of_point);
        feature_points->channels.push_back(v_of_point);
        feature_points->channels.push_back(velocity_x_of_point);
        feature_points->channels.push_back(velocity_y_of_point);
        ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());

        // 由于第一帧无法计算光流速度,跳过第一帧
        if (!init_pub)
        {
            init_pub = 1;
        }
        else
            pub_img.publish(feature_points);

        ROS_INFO("=========Feature published!===========");
        if (SHOW_TRACK)
        {
            ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);
            cv::Mat stereo_img = ptr->image;
            e_ptr = cv_bridge::cvtColor(e_ptr, sensor_msgs::image_encodings::BGR8);
            cv::Mat ev_img = e_ptr->image;

            for (int i = 0; i < NUM_OF_CAM; i++)
            {
                if (i == 0)
                {
                    // cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
                    cv::Mat tmp_img = stereo_img.rowRange(0, ROW);
                    cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);

                    // 显示特征跟踪状态, len越小越好, 跟踪点越红, len越大越差, 跟踪点越蓝
                    for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
                    {
                        double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
                        cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
                    }
                    pub_match.publish(ptr->toImageMsg());
                }
                else if (i == 1)
                {
                    cv::Mat tmp_img = ev_img.rowRange(0, ROW);
                    cv::cvtColor(e_show_img, tmp_img, CV_GRAY2RGB);

                    // 显示特征跟踪状态, len越小越好, 跟踪点越红, len越大越差, 跟踪点越蓝
                    for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
                    {
                        double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
                        cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
                    }
                    pub_match_event.publish(e_ptr->toImageMsg());
                }
            }
        }
        ROS_INFO("=========Image published!===========");
    }
    ROS_INFO("=========Callback ended!===========");
}

int main(int argc, char **argv)
{
    // ros初始化和设置句柄
    ros::init(argc, argv, "feature_tracker");
    ros::NodeHandle n("~");

    // 设置logger的级别, 只有级别大于或等于level的日志记录消息才会得到处理
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

    // 读取yaml中的一些配置参数
    readParameters(n);

    // 读取每个相机实例对应的相机内参
    for (int i = 0; i < NUM_OF_CAM; i++)
        // trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);
        trackerData[i].readIntrinsicParameter(CAM_NAMES[0]);

    // 判断是否加入鱼眼mask来去除边缘噪声
    if (FISHEYE)
    {
        for (int i = 0; i < NUM_OF_CAM; i++)
        {
            trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
            if (!trackerData[i].fisheye_mask.data)
            {
                ROS_INFO("load mask fail");
                ROS_BREAK();
            }
            else
                ROS_INFO("load mask success");
        }
    }
    
    // **********************************************************************
    message_filters::Subscriber<sensor_msgs::Image> sub_img, sub_event;
    sub_img.subscribe(n, IMAGE_TOPIC, 10, ros::TransportHints().tcpNoDelay());
    sub_event.subscribe(n, EVENT_TOPIC, 10, ros::TransportHints().tcpNoDelay());
    typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
    std::unique_ptr<message_filters::Synchronizer<MySyncPolicy>> sync_;
    sync_.reset(new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(100), sub_img, sub_event));
    ROS_INFO("=========Entering main function===========");
    sync_->registerCallback(boost::bind(&imgCb, _1, _2));
    // **********************************************************************

    // 发布跟踪的特征点feature，用于后端优化
    pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
    // 发布特征点匹配feature_img，用于RVIZ和调试
    pub_match = n.advertise<sensor_msgs::Image>("feature_img", 1000);
    pub_match_event = n.advertise<sensor_msgs::Image>("feature_event", 1000);
    //发布restart
    pub_restart = n.advertise<std_msgs::Bool>("restart", 1000);

    ros::spin();
    return 0;
}